using BepuPhysics.Collidables;
using BepuPhysics.CollisionDetection;
using BepuUtilities;
using BepuUtilities.Collections;
using BepuUtilities.Memory;
using System.Diagnostics;
using System.Runtime.CompilerServices;
#if MYCODE
using BepuUtilities.Vectors;
#else
using System.Numerics;
#endif
namespace BepuPhysics
{
    public struct BoundsContinuation
    {
        // 位0-30：正文索引
        // 第31位：复合标志;如果置位,则应将延续合并到目标槽中,而不是仅将其置位。
        uint packed;

        /// <summary>
        /// 获取与此延续关联的正文的索引。
        /// </summary>
        public int BodyIndex
        {
            [MethodImpl(MethodImplOptions.AggressiveInlining)]
            get
            {
                return (int)(packed & 0x7FFFFFFF);
            }
        }

        /// <summary>
        /// 获取此延续是否与复合的子项关联。
        /// </summary>
        public bool CompoundChild
        {
            [MethodImpl(MethodImplOptions.AggressiveInlining)]
            get
            {
                return (packed & (1u << 31)) > 0;
            }
        }

        /// <summary>
        /// 为给定的非复合体创建边界框计算连续性。
        /// </summary>
        /// <param name="bodyIndex">要设置其边界框的正文索引。</param>
        public static BoundsContinuation CreateContinuation(int bodyIndex)
        {
            Debug.Assert(bodyIndex >= 0);
            BoundsContinuation toReturn;
            toReturn.packed = (uint)bodyIndex;
            return toReturn;
        }
        /// <summary>
        /// 为给定复合体创建边界框计算延续。
        /// </summary>
        /// <param name="compoundBodyIndex">要设置边界框的复合体的索引。</param>
        public static BoundsContinuation CreateCompoundChildContinuation(int compoundBodyIndex)
        {
            Debug.Assert(compoundBodyIndex >= 0);
            BoundsContinuation toReturn;
            toReturn.packed = (1u << 31) | (uint)compoundBodyIndex;
            return toReturn;
        }
    }


    public struct BoundingBoxInstance
    {
        public RigidPose Pose;
        public BodyVelocity Velocities;
        public int ShapeIndex;
        public BoundsContinuation Continuation;
    }

    public struct BoundingBoxInstanceWide<TShape, TShapeWide> where TShape : unmanaged, IShape where TShapeWide : unmanaged, IShapeWide<TShape>
    {
        public TShapeWide Shape;
        public Vector<float> MaximumExpansion;
        public RigidPoses Pose;
        public BodyVelocities Velocities;
    }

    public struct BoundingBoxBatcher
    {
        internal BufferPool pool;
        internal Shapes shapes;
        internal Bodies bodies;
        internal BroadPhase broadPhase;
        internal float dt;

        int minimumBatchIndex, maximumBatchIndex;
        Buffer<QuickList<BoundingBoxInstance>> batches;

        /// <summary>
        /// 在执行AABB更新之前,每种类型要累计的正文数。每批处理的主体越多,虚拟开销和执行差异就越小。
        /// 但是,这应该保持在足够低的水平,以便边界框更新必须收集的数据通常仍在L1中。
        /// </summary>
        public const int CollidablesPerFlush = 16;

        public unsafe BoundingBoxBatcher(Bodies bodies, Shapes shapes, BroadPhase broadPhase, BufferPool pool, float dt)
        {
            this.bodies = bodies;
            this.shapes = shapes;
            this.broadPhase = broadPhase;
            this.pool = pool;
            this.dt = dt;
            pool.TakeAtLeast(shapes.RegisteredTypeSpan, out batches);
            // 需要清除以确保我们知道何时需要创建批次以及何时需要处置批次。
            batches.Clear(0, shapes.RegisteredTypeSpan);
            minimumBatchIndex = shapes.RegisteredTypeSpan;
            maximumBatchIndex = -1;
        }

        public unsafe void ExecuteConvexBatch<TShape, TShapeWide>(ConvexShapeBatch<TShape, TShapeWide> shapeBatch) where TShape : unmanaged, IConvexShape where TShapeWide : unmanaged, IShapeWide<TShape>
        {
            var instanceBundle = default(BoundingBoxInstanceWide<TShape, TShapeWide>);
            if (instanceBundle.Shape.InternalAllocationSize > 0) // TODO：检查以确保JIT省略了分支。
            {
                var memory = stackalloc byte[instanceBundle.Shape.InternalAllocationSize];
                instanceBundle.Shape.Initialize(new RawBuffer(memory, instanceBundle.Shape.InternalAllocationSize));
            }
            ref var batch = ref batches[shapeBatch.TypeId];
            ref var instancesBase = ref batch[0];
            ref var activeSet = ref bodies.ActiveSet;

            for (int bundleStartIndex = 0; bundleStartIndex < batch.Count; bundleStartIndex += Vector<float>.Count)
            {
                int countInBundle = batch.Count - bundleStartIndex;
                if (countInBundle > Vector<float>.Count)
                    countInBundle = Vector<float>.Count;
                ref var bundleInstancesStart = ref Unsafe.Add(ref instancesBase, bundleStartIndex);
                // 请注意,对于某些形状类型,执行聚集-散布以启用矢量化包执行是不值得的。
                // 为了简单起见,我们只是忽略了这一事实。总体而言,边界框更新并不是一个大问题。也就是说,
                // 如果要进一步优化这一点,形状批次可以基于每个类型在矢量化和无聚集标量实现之间进行选择。
                for (int innerIndex = 0; innerIndex < countInBundle; ++innerIndex)
                {
                    ref var instance = ref Unsafe.Add(ref bundleInstancesStart, innerIndex);
                    ref var targetInstanceSlot = ref GatherScatter.GetOffsetInstance(ref instanceBundle, innerIndex);
                    // 此属性应为常量值,并且JIT具有类型知识,因此此分支应进行优化。
                    if (instanceBundle.Shape.AllowOffsetMemoryAccess)
                        targetInstanceSlot.Shape.WriteFirst(shapeBatch.shapes[instance.ShapeIndex]);
                    else
                        instanceBundle.Shape.WriteSlot(innerIndex, shapeBatch.shapes[instance.ShapeIndex]);
                    Vector3Wide.WriteFirst(instance.Pose.Position, ref targetInstanceSlot.Pose.Position);
                    QuaternionWide.WriteFirst(instance.Pose.Orientation, ref targetInstanceSlot.Pose.Orientation);
                    Vector3Wide.WriteFirst(instance.Velocities.Linear, ref targetInstanceSlot.Velocities.Linear);
                    Vector3Wide.WriteFirst(instance.Velocities.Angular, ref targetInstanceSlot.Velocities.Angular);
                    ref var collidable = ref activeSet.Collidables[instance.Continuation.BodyIndex];
                    GatherScatter.GetFirst(ref targetInstanceSlot.MaximumExpansion) =
                        collidable.Continuity.AllowExpansionBeyondSpeculativeMargin ? float.MaxValue : collidable.SpeculativeMargin;
                }
                instanceBundle.Shape.GetBounds(ref instanceBundle.Pose.Orientation, countInBundle, out var maximumRadius, out var maximumAngularExpansion, out var bundleMin, out var bundleMax);
                BoundingBoxHelpers.ExpandBoundingBoxes(ref bundleMin, ref bundleMax, ref instanceBundle.Velocities, dt,
                    ref maximumRadius, ref maximumAngularExpansion, ref instanceBundle.MaximumExpansion);
                // TODO：请注意,如果更改姿势表示,则必须更新该区域。
                Vector3Wide.Add(instanceBundle.Pose.Position, bundleMin, out bundleMin);
                Vector3Wide.Add(instanceBundle.Pose.Position, bundleMax, out bundleMax);

                for (int innerIndex = 0; innerIndex < countInBundle; ++innerIndex)
                {
                    ref var instance = ref Unsafe.Add(ref bundleInstancesStart, innerIndex);
                    broadPhase.GetActiveBoundsPointers(activeSet.Collidables[instance.Continuation.BodyIndex].BroadPhaseIndex, out var minPointer, out var maxPointer);
                    ref var sourceBundleMin = ref GatherScatter.GetOffsetInstance(ref bundleMin, innerIndex);
                    ref var sourceBundleMax = ref GatherScatter.GetOffsetInstance(ref bundleMax, innerIndex);
                    // 请注意,如果主体是复合的,我们将与现有边界框合并。这需要将化合物初始化为(maxvalue,-maxvalue)。
                    // TODO：我们在相当复杂的问题上咬紧牙关,以避免在非化合物上合并。总体来说,简单地合并所有身体可能会更好。当然更简单。
                    // 值得检查一下性能;如果检测不到,就换成更简单的版本。
                    if (instance.Continuation.CompoundChild)
                    {
                        var min = new Vector3(sourceBundleMin.X[0], sourceBundleMin.Y[0], sourceBundleMin.Z[0]);
                        var max = new Vector3(sourceBundleMax.X[0], sourceBundleMax.Y[0], sourceBundleMax.Z[0]);
                        BoundingBox.CreateMerged(*minPointer, *maxPointer, min, max, out *minPointer, out *maxPointer);
                    }
                    else
                    {
                        *minPointer = new Vector3(sourceBundleMin.X[0], sourceBundleMin.Y[0], sourceBundleMin.Z[0]);
                        *maxPointer = new Vector3(sourceBundleMax.X[0], sourceBundleMax.Y[0], sourceBundleMax.Z[0]);
                    }
                }
            }
        }

        public unsafe void ExecuteHomogeneousCompoundBatch<TShape, TChildShape, TChildShapeWide>(HomogeneousCompoundShapeBatch<TShape, TChildShape, TChildShapeWide> shapeBatch)
            where TShape : unmanaged, IHomogeneousCompoundShape<TChildShape, TChildShapeWide>
            where TChildShape : IConvexShape
            where TChildShapeWide : IShapeWide<TChildShape>
        {
            ref var batch = ref batches[shapeBatch.TypeId];
            ref var activeSet = ref bodies.ActiveSet;
            for (int i = 0; i < batch.Count; ++i)
            {
                ref var instance = ref batch[i];
                var bodyIndex = instance.Continuation.BodyIndex;
                ref var collidable = ref activeSet.Collidables[bodyIndex];
                broadPhase.GetActiveBoundsPointers(collidable.BroadPhaseIndex, out var min, out var max);
                var maximumAllowedExpansion = collidable.Continuity.AllowExpansionBeyondSpeculativeMargin ? float.MaxValue : collidable.SpeculativeMargin;
                shapeBatch[instance.ShapeIndex].ComputeBounds(instance.Pose.Orientation, out *min, out *max);
                // 假设动态网格极为罕见,而角速度极高的动态网格更为罕见,
                // 我们只会用一个简单的角膨胀上限。这大大简化了网格边界框的计算(没有点积)。
                var absMin = Vector3.Abs(*min);
                var absMax = Vector3.Abs(*max);
                var maximumRadius = Vector3.Max(absMin, absMax).Length();

                var minimumComponents = Vector3.Min(absMin, absMax);
                var minimumRadius = MathHelper.Min(minimumComponents.X, MathHelper.Min(minimumComponents.Y, minimumComponents.Z));
                BoundingBoxHelpers.ExpandBoundingBox(ref *min, ref *max, instance.Velocities.Linear, instance.Velocities.Angular, dt, maximumRadius, maximumRadius - minimumRadius, maximumAllowedExpansion);
                *min += instance.Pose.Position;
                *max += instance.Pose.Position;
            }
        }

        public unsafe void ExecuteCompoundBatch<TShape>(CompoundShapeBatch<TShape> shapeBatch) where TShape : unmanaged, ICompoundShape
        {
            ref var batch = ref batches[shapeBatch.TypeId];
            ref var activeSet = ref bodies.ActiveSet;
            var minValue = new Vector3(float.MaxValue);
            var maxValue = new Vector3(-float.MaxValue);
            for (int i = 0; i < batch.Count; ++i)
            {
                ref var instance = ref batch[i];
                var bodyIndex = instance.Continuation.BodyIndex;
                // 我们必须清除化合物使用的界限,因为每个贡献机构都会将它们的贡献合并为整体。
                broadPhase.GetActiveBoundsPointers(activeSet.Collidables[bodyIndex].BroadPhaseIndex, out var min, out var max);
                *min = minValue;
                *max = maxValue;
                shapeBatch.shapes[instance.ShapeIndex].AddChildBoundsToBatcher(ref this, instance.Pose, instance.Velocities, bodyIndex);
            }
        }


        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        void Add(TypedIndex shapeIndex, in RigidPose pose, in BodyVelocity velocity, BoundsContinuation continuation)
        {
            var typeIndex = shapeIndex.Type;
            Debug.Assert(typeIndex >= 0 && typeIndex < batches.Length, "The preallocated type batch array should be able to hold every type index. Is the type index broken?");
            ref var batchSlot = ref batches[typeIndex];
            if (!batchSlot.Span.Allocated)
            {
                // 尚不存在此类型的列表。
                batchSlot = new QuickList<BoundingBoxInstance>(CollidablesPerFlush, pool);
                if (typeIndex < minimumBatchIndex)
                    minimumBatchIndex = typeIndex;
                if (typeIndex > maximumBatchIndex)
                    maximumBatchIndex = typeIndex;
            }
            // TODO：可以说,拼凑化合物是愚蠢的。
            // 从技术上讲,它为向量化他们的孩子姿势计算打开了大门,但几乎可以肯定的是,这些计算无论如何都不值得矢量化。
            // 可能要考虑在这里直接触发化合物的边界调度。
            // (不过,这样做会更复杂。)
            ref var instance = ref batchSlot.AllocateUnsafely();
            var shapeBatch = shapes[typeIndex];
            instance.Pose = pose;
            instance.Velocities = velocity;
            instance.ShapeIndex = shapeIndex.Index;
            instance.Continuation = continuation;

            if (batchSlot.Count == CollidablesPerFlush)
            {
                shapeBatch.ComputeBounds(ref this);
                batchSlot.Count = 0;
            }
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public void Add(int bodyIndex, in RigidPose pose, in BodyVelocity velocity, in Collidable collidable)
        {
            // 为方便起见,此函数处理可碰撞引用指向零的情况。
            // 请注意,这涉及到与完全可碰撞相关联的内存。没关系-如果它有一个可碰撞的,我们很快就会读到它的睡觉。
            // 从技术上讲,您可以进行第二次遍历,只处理可碰撞的对象,而不是遍历所有实体并执行最后一秒的分支。
            // 但然后您将从缓存L1/L2中逐出所有内容。99.99%的情况下,物体都会有形状,所以这不是一个很难预测的分支。
            // 即使是50%,与刚接触的数据源一起执行的缓存优势也会超过错误预测。
            if (collidable.Shape.Exists)
            {
                Add(collidable.Shape, pose, velocity, BoundsContinuation.CreateContinuation(bodyIndex));
            }
        }
        public void AddCompoundChild(int bodyIndex, TypedIndex shapeIndex, in RigidPose pose, in BodyVelocity velocity)
        {
            Add(shapeIndex, pose, velocity, BoundsContinuation.CreateCompoundChildContinuation(bodyIndex));
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public void Flush()
        {
#if DEBUG
            // 我们依赖于所分配的形状类型ID,使得化合物和凸面相对于它们的ID被分成两个连续的组。
            // 这避免了功生成中的循环,因为化合物只能包含凸面。由于任务图是非循环的,我们可以在一遍中刷新。
            bool compoundEncountered = false;
            for (int i = minimumBatchIndex; i <= maximumBatchIndex; ++i)
            {
                var batch = shapes[i];
                if (batch != null)
                {
                    if (compoundEncountered)
                    {
                        Debug.Assert(batch.Compound, "If a compound shape batch has been found, all subsequent batches must also be compound to avoid cycles.");
                    }
                    else if (batch.Compound)
                    {
                        compoundEncountered = true;
                    }
                }
            }
#endif
            // 注意反向迭代。首先执行所有复合批处理。
            for (int i = maximumBatchIndex; i >= minimumBatchIndex; --i)
            {
                ref var batch = ref batches[i];
                if (batch.Count > 0)
                {
                    shapes[i].ComputeBounds(ref this);
                }
                // 处理批处理和任何相关的缓冲区;因为刷新是一遍,所以我们不再需要这样做。
                if (batch.Span.Allocated)
                {
                    batch.Dispose(pool);
                }
            }
            pool.Return(ref batches);
        }
    }
}
